﻿#region COPYRIGHT
//--------------------------------------------------------------------------------
// <copyright file="QuaternionExtensions.cs" company="starLiGHT Entertainment Studios">
//    Copyright (c) 2007, 2008, 2009, 2010 
//       Roland Rosenkranz (Glatzemann@email.de)
// </copyright>
// <license>
//   This file is part of starLiGHT.Core.
//
//   starLiGHT.Core is free software: you can redistribute it and/or modify
//   it under the terms of the GNU Lesser General Public License as published by
//   the Free Software Foundation, either version 3 of the License, or
//   (at your option) any later version.
//
//   starLiGHT.Core is distributed in the hope that it will be useful,
//   but WITHOUT ANY WARRANTY; without even the implied warranty of
//   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//   GNU Lesser General Public License for more details.
//
//   You should have received a copy of the GNU Lesser General Public License
//   along with starLiGHT.Core. If not, see http://www.gnu.org/licenses/.
//    
//   ADDITIONAL (commercial) LICENSES for starLiGHT.Core are available on request.
// </license>
// <version>
// **************[ starLiGHT.Engine SVN ]**********************
// * $Rev:: 620                   $:  Revision of last commit *
// * $Author:: unknown            $:  Author of last commit   *
// * $Date:: 2010-12-07 15:23:58 #$:  Date of last commit     *
// ************************************************************
// </version>
//--------------------------------------------------------------------------------
#endregion

namespace starLiGHT
{
    #region Using Statements
    using System;
    using System.Collections.Generic;
    using System.Text;
    using Microsoft.Xna.Framework;

    #endregion

    /// <summary>
    /// Extension methods for the Quaternion data type
    /// </summary>
    public static class QuaternionExtensions
    {
        public static Matrix ToRotationMatrix(this Quaternion q)
        {
            Matrix ret = Matrix.Identity;

            float tx = 2.0f * q.X;
            float ty = 2.0f * q.Y;
            float tz = 2.0f * q.Z;
            float twx = tx * q.W;
            float twy = ty * q.W;
            float twz = tz * q.W;
            float txx = tx * q.X;
            float txy = ty * q.X;
            float txz = tz * q.X;
            float tyy = ty * q.Y;
            float tyz = tz * q.Y;
            float tzz = tz * q.Z;

            ret.M11 = 1.0f - (tyy + tzz);
            ret.M12 = txy - twz;
            ret.M13 = txz + twy;
            ret.M21 = txy + twz;
            ret.M22 = 1.0f - (txx + tzz);
            ret.M23 = tyz - twx;
            ret.M31 = txz - twy;
            ret.M32 = tyz + twx;
            ret.M33 = 1.0f - (txx + tyy);

            return ret;
        }

        public static Vector3[] ToAxes(this Quaternion q)
        {
            Vector3[] ret = new Vector3[3];
            Matrix rot = q.ToRotationMatrix();

            for (int col = 0; col < 3; col++)
            {
                ret[col].X = rot.Item(0, col);
                ret[col].Y = rot.Item(1, col);
                ret[col].Z = rot.Item(2, col);
            }

            return ret;
        }

        public static Quaternion CreateFromRotationMatrix(this Quaternion q, Matrix rot)
        {
            //// Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
            //// article "Quaternion Calculus and Fast Animation".

            float trace = rot.M11 + rot.M22 + rot.M33;
            float root;

            if (trace > 0.0f)
            {
                // |w| > 1/2, may as well choose w > 1/2
                root = (float)Math.Sqrt(trace + 1.0f);  // 2w
                q.W = 0.5f * root;
                root = 0.5f / root;  // 1/(4w)
                q.X = (rot.M32 - rot.M23) * root;
                q.Y = (rot.M13 - rot.M31) * root;
                q.Z = (rot.M21 - rot.M12) * root;
            }
            else
            {
                // |w| <= 1/2
                int[] next = { 1, 2, 0 };
                int i = 0;
                if (rot.M22 > rot.M11)
                {
                    i = 1;
                }

                if (rot.M33 > rot.Item(i, i))
                {
                    i = 2;
                }

                int j = next[i];
                int k = next[j];

                root = (float)Math.Sqrt(rot.Item(i, i) - rot.Item(j, j) - rot.Item(k, k) + 1.0);
                float[] apkQuat = { q.X, q.Y, q.Z };
                apkQuat[i] = 0.5f * root;
                root = 0.5f / root;
                q.W = (rot.Item(k, j) - rot.Item(j, k)) * root;
                apkQuat[j] = (rot.Item(j, i) + rot.Item(i, j)) * root;
                apkQuat[k] = (rot.Item(k, i) + rot.Item(i, k)) * root;

                q.X = apkQuat[0];
                q.Y = apkQuat[1];
                q.Z = apkQuat[2];
            }

            return q;
        }

        public static Quaternion CreateFromAxes(this Quaternion q, Vector3 xaxis, Vector3 yaxis, Vector3 zaxis)
        {
            Matrix rot = new Matrix();

            rot.M11 = xaxis.X;
            rot.M21 = xaxis.Y;
            rot.M31 = xaxis.Z;

            rot.M12 = yaxis.X;
            rot.M22 = yaxis.Y;
            rot.M32 = yaxis.Z;

            rot.M13 = zaxis.X;
            rot.M23 = zaxis.Y;
            rot.M33 = zaxis.Z;

            q = Quaternion.CreateFromRotationMatrix(rot);

            return q;
        }

        public static Vector3 Multiply(this Quaternion q, Vector3 v)
        {
            // nVidia SDK implementation
            Vector3 uv, uuv;
            Vector3 qvec = new Vector3(q.X, q.Y, q.Z);
            uv = Vector3.Cross(qvec, v);
            uuv = Vector3.Cross(qvec, uv);
            uv *= 2.0f * q.W;
            uuv *= 2.0f;

            return v + uv + uuv;
        }
    }
}
